/*
 * @Author: LVGRAPE
 * @LastEditors: LVGRAPE
 */

#include "Icm426xxTransport.h"
#include "Icm426xxDriver_HL.h"
#include "rtthread.h"
#include "rtdevice.h"
#include <drv_spi.h>
#include "zino.h"

#define DBG_TAG "sensor.icm42688p"
#define DBG_LVL DBG_LOG
#include <rtdbg.h>

#define SERIF_TYPE ICM426XX_UI_SPI4
// NOTE - 测试板用的qmi的总线,正常板子用的lsm的总线
#define ICM42688P_BUS_NAME "spi_icm"
#define ICM_I2C_ADDR 0x68
#define USE_CLK_IN 0
#define SENSOR_ODR 0

const float gyro_full_scale_factor[8] = {
    16.4f,   //+/-2000dps
    32.8f,   //+/-1000dps
    65.5f,   //+/-500dps
    131.f,   //+/-250dps
    256.f,   //+/-125dps
    524.3f,  //+/-62.5dps
    1048.6f, //+/-31.25dps
    2097.2f, //+/-15.625dsp
};

const uint16_t accel_full_scale_factor[4] = {
    2048, //+/-16g
    4096, //+/-8g
    8192, //+/-4g
    16384 //+/-2g
};

// static struct inv_icm426xx_serif icm42688p_dev;
/* Just a handy variable to handle the icm426xx object */
// static struct inv_icm426xx icm_driver;
/* Just a handy variable to handle the spi object */
// static struct rt_spi_device *icm6288p_spi = RT_NULL;
/* Just a handy variable to handle the i2c object */
// static struct rt_i2c_bus_device *icm6288p_i2c = RT_NULL;
/* Just a handy variable to handle the accel full scale factor */
static uint16_t accel_fs_factor = 2048;
/* Just a handy variable to handle the gyro full scale factor */
static float gyro_fs_factor = 16.4f;

/**
 * @brief 读寄存器
 *
 * @param serif
 * @param reg
 * @param rbuffer
 * @param rlen
 * @return int
 */
int io_hal_read_reg(struct inv_icm426xx_serif *serif, uint8_t reg, uint8_t *rbuffer,
                    uint32_t rlen)
{
    switch (serif->serif_type)
    {
    case ICM426XX_AUX1_SPI3:
    case ICM426XX_AUX1_SPI4:
    case ICM426XX_AUX2_SPI3:
    case ICM426XX_UI_SPI4:
        struct rt_spi_device *icm6288p_spi = (struct rt_spi_device *)serif->context;
        reg |= 0x80;
        return rt_spi_send_then_recv(icm6288p_spi, &reg, 1, rbuffer, rlen);
    case ICM426XX_UI_I2C:
    {
        struct rt_i2c_bus_device *icm6288p_i2c = (struct rt_i2c_bus_device *)serif->context;
        struct rt_i2c_msg msg[2];
        msg[0].addr = ICM_I2C_ADDR;
        msg[0].buf = &reg;
        msg[0].len = 1;
        msg[0].flags = RT_I2C_WR;

        msg[1].addr = ICM_I2C_ADDR;
        msg[1].buf = rbuffer;
        msg[1].len = rlen;
        msg[1].flags = RT_I2C_RD | RT_I2C_NO_START;
        return rt_i2c_transfer(icm6288p_i2c, msg, 2);
    }
    default:
        return -1;
    }
}
/**
 * @brief 写寄存器
 *
 * @param serif
 * @param reg
 * @param wbuffer
 * @param wlen
 * @return int
 */
int io_hal_write_reg(struct inv_icm426xx_serif *serif, uint8_t reg, const uint8_t *wbuffer,
                     uint32_t wlen)
{
    int rc;

    switch (serif->serif_type)
    {
    case ICM426XX_AUX1_SPI3:
    case ICM426XX_AUX1_SPI4:
    case ICM426XX_AUX2_SPI3:
    case ICM426XX_UI_SPI4:
        struct rt_spi_device *icm6288p_spi = (struct rt_spi_device *)serif->context;
        // RT_ASSERT(icm6288p_spi != RT_NULL);
        for (uint32_t i = 0; i < wlen; i++)
        {
            uint8_t regAddr = reg + i;
            rc = rt_spi_send_then_send(icm6288p_spi, &regAddr, 1, &wbuffer[i], 1);
            if (rc)
                return rc;
        }
        return 0;
    case ICM426XX_UI_I2C:
    {
        struct rt_i2c_bus_device *icm6288p_i2c = (struct rt_i2c_bus_device *)serif->context;
        // RT_ASSERT(icm6288p_i2c != RT_NULL);
        struct rt_i2c_msg msg[2];
        msg[0].addr = ICM_I2C_ADDR;
        msg[0].buf = &reg;
        msg[0].len = 1;
        msg[0].flags = RT_I2C_WR;

        msg[1].addr = ICM_I2C_ADDR;
        msg[1].buf = (uint8_t *)wbuffer;
        msg[1].len = wlen;
        msg[1].flags = RT_I2C_WR | RT_I2C_NO_START;
        return rt_i2c_transfer(icm6288p_i2c, msg, 2);
    }
    default:
        return -1;
    }
}
/**
 * @brief 初始化spi,i2c接口给icm42688用，i2c没有调试
 *
 * @param icm_serif
 * @return int
 */
static int icm42688_platform_init(struct inv_icm426xx_serif *icm_serif, uint32_t serif_type, const char *bus_name)
{
    rt_device_t bus = rt_device_find(bus_name);
    if (bus == RT_NULL)
    {
        LOG_E("can't find %s bus!", bus_name);
        return -RT_ERROR;
    }

    if (serif_type == ICM426XX_UI_SPI4)
    {
        struct rt_spi_device *icm6288p_spi = (struct rt_spi_device *)bus;
        icm6288p_spi->config.max_hz = 20000000;
        icm6288p_spi->config.data_width = 8;
        icm6288p_spi->config.mode = RT_SPI_MODE_3 | RT_SPI_MSB;
        LOG_D("%s spi init ok!", bus_name);
    }

    /* Initialize serial inteface between MCU and Icm426xx */
    icm_serif->context = bus;
    icm_serif->read_reg = io_hal_read_reg;
    icm_serif->write_reg = io_hal_write_reg;
    icm_serif->max_read = 1024 * 32;  /* maximum number of bytes allowed per serial read */
    icm_serif->max_write = 1024 * 32; /* maximum number of bytes allowed per serial write */
    icm_serif->serif_type = serif_type;

    return RT_EOK;
}
void inv_icm426xx_sleep_us(uint32_t us)
{
    if (us < 1000)
        rt_hw_us_delay(us);
    else
        rt_thread_mdelay(us / 1000);
}
/**
 * @brief 初始化：上电，验证id
 *
 * @param icm_serif
 * @return int
 */
int icm42688p_SetupInvDevice(struct inv_icm426xx_serif *icm_serif, struct inv_icm426xx *s)
{
    int rc = 0;
    uint8_t who_am_i;

    /* Initialize device */
    LOG_I("Initializing Icm42688p");

    rc = inv_icm426xx_init(s, icm_serif, NULL);
    if (rc != RT_EOK)
    {
        LOG_E("!!! ERROR : failed to initialize Icm426xx. err:%d",rc);
        return rc;
    }

    /* Check WHOAMI */
    LOG_I("Check Icm426xx whoami value");

    rc = inv_icm426xx_get_who_am_i(s, &who_am_i);
    if (rc != INV_ERROR_SUCCESS)
    {
        LOG_E("!!! ERROR : failed to read Icm426xx whoami value.");
        return rc;
    }

    if (who_am_i != ICM_WHOAMI)
    {
        LOG_E("!!! ERROR :  bad WHOAMI value. Got 0x%02x (expected: 0x%02x)",
              who_am_i, ICM_WHOAMI);
        return INV_ERROR;
    }
    return rc;
}
/**
 * @brief 配置量程，ODR, LFP
 *
 * @return int
 */
int icm42688p_ConfigureInvDevice(struct inv_icm426xx *s)
{
    int rc = 0;
    uint8_t data;
    /* Set Pulse to support high rates */
    rc |= inv_icm426xx_read_reg(s, MPUREG_INT_CONFIG1, 1, &data);
    data |=
        ((uint8_t)ICM426XX_INT_TPULSE_DURATION_8_US) | ((uint8_t)ICM426XX_INT_TDEASSERT_DISABLED);
    rc |= inv_icm426xx_write_reg(s, MPUREG_INT_CONFIG1, 1, &data);

    /* Disable fifo usage, data will be read from sensors registers*/
    rc |= inv_icm426xx_configure_fifo(s, INV_ICM426XX_FIFO_DISABLED);
    if (rc != INV_ERROR_SUCCESS)
    {
        LOG_E("!!! ERROR : failed to initialize Icm426xx.");
        return rc;
    }

#if defined(ICM42633) && USE_CLK_IN
    /*
     * ICM42633 is a triple interface device. To access CLKIN, AUX2 interface needs to be disabled.
     * Use INV_ICM426XX_DUAL_INTERFACE mode. The following mode are also compatible:
     *  - INV_ICM426XX_SINGLE_INTERFACE
     *  - INV_ICM426XX_DUAL_INTERFACE_SPI4
     */
    rc |= inv_icm426xx_interface_change_procedure(s, INV_ICM426XX_DUAL_INTERFACE);
#endif
    rc |= inv_icm426xx_enable_clkin_rtc(s, USE_CLK_IN);

    rc |= inv_icm426xx_set_accel_fsr(s, ICM426XX_ACCEL_CONFIG0_FS_SEL_8g);
    rc |= inv_icm426xx_set_gyro_fsr(s, ICM426XX_GYRO_CONFIG0_FS_SEL_2000dps);

    // rc |= inv_icm426xx_set_accel_frequency(s, ICM426XX_ACCEL_CONFIG0_ODR_200_HZ);
    // rc |= inv_icm426xx_set_gyro_frequency(s, ICM426XX_GYRO_CONFIG0_ODR_200_HZ);
    rc |= inv_icm426xx_set_accel_frequency(s, ICM426XX_ACCEL_CONFIG0_ODR_1_KHZ);
    rc |= inv_icm426xx_set_gyro_frequency(s, ICM426XX_GYRO_CONFIG0_ODR_1_KHZ);

    // #if SENSOR_ODR == HIGH_RATE_8KHZ
    //     rc |= inv_icm426xx_set_accel_frequency(s, ICM426XX_ACCEL_CONFIG0_ODR_8_KHZ);
    //     rc |= inv_icm426xx_set_gyro_frequency(s, ICM426XX_GYRO_CONFIG0_ODR_8_KHZ);
    // #elif SENSOR_ODR == HIGH_RATE_16KHZ
    //     rc |= inv_icm426xx_set_accel_frequency(s, ICM426XX_ACCEL_CONFIG0_ODR_16_KHZ);
    //     rc |= inv_icm426xx_set_gyro_frequency(s, ICM426XX_GYRO_CONFIG0_ODR_16_KHZ);
    // #elif SENSOR_ODR == HIGH_RATE_32KHZ
    //     rc |= inv_icm426xx_set_accel_frequency(s, ICM426XX_ACCEL_CONFIG0_ODR_32_KHZ);
    //     rc |= inv_icm426xx_set_gyro_frequency(s, ICM426XX_GYRO_CONFIG0_ODR_32_KHZ);
    // #endif
    // NOTE 开启低噪声模式
    rc |= inv_icm426xx_enable_gyro_low_noise_mode(s);
    rc |= inv_icm426xx_enable_accel_low_noise_mode(s);

    // rc |= inv_icm426xx_set_accel_ln_bw(s, ICM426XX_GYRO_ACCEL_CONFIG0_GYRO_FILT_BW_20);
    //  rc |= inv_icm426xx_set_gyro_ln_bw(s, ICM426XX_GYRO_ACCEL_CONFIG0_GYRO_FILT_BW_40);

    // NOTE 读数量程系数，原始数据除以这个数能得真实值
    ICM426XX_ACCEL_CONFIG0_FS_SEL_t accel_fsr;
    ICM426XX_GYRO_CONFIG0_FS_SEL_t gyro_fsr;
    rc |= inv_icm426xx_get_accel_fsr(s, &accel_fsr);
    rc |= inv_icm426xx_get_gyro_fsr(s, &gyro_fsr);
    accel_fs_factor = accel_full_scale_factor[(accel_fsr >> BIT_ACCEL_CONFIG0_FS_SEL_POS) % 4];
    gyro_fs_factor = gyro_full_scale_factor[(gyro_fsr >> BIT_GYRO_CONFIG0_FS_SEL_POS) % 8];
    LOG_D("accel_fs_factor:%d", accel_fs_factor);
    LOG_D("gyro_fs_factor:%d.%01d", (int)gyro_fs_factor, (int)(gyro_fs_factor * 10) % 10);

    /* Wait the maximum startup time in case gyro is enbaled */
    // inv_icm426xx_sleep_us(ICM426XX_GYR_STARTUP_TIME_US);

    return rc;
}
uint64_t inv_icm426xx_get_time_us(void)
{
    return rt_tick_get_millisecond() * 1000;
}

/**
 * @brief 配置icm42688,上电，量程，odr
 *
 * @return int
 */
struct inv_icm426xx *icm42688p_create(void)
{
    rt_thread_mdelay(25); // NOTE waite for power stable
    struct inv_icm426xx_serif *icm_serif = rt_calloc(1, sizeof(struct inv_icm426xx_serif));
    struct inv_icm426xx *icm42688p_dev = rt_calloc(1, sizeof(struct inv_icm426xx));
    if (!icm_serif || !icm42688p_dev)
    {
        LOG_E("rt_calloc fail!");
        return NULL;
    }
    if (icm42688_platform_init(icm_serif, SERIF_TYPE, ICM42688P_BUS_NAME) == RT_EOK)
    {
        LOG_I("platform init ok!");
        if (icm42688p_SetupInvDevice(icm_serif, icm42688p_dev) == RT_EOK)
        {
            LOG_I("SetupInvDevice ok!");
            if (icm42688p_ConfigureInvDevice(icm42688p_dev) == RT_EOK)
            {
                LOG_I("ConfigureInvDevice ok!");
                return icm42688p_dev;
            }
            else
            {
                LOG_E("ConfigureInvDevice fail!");
            }
        }
        else
        {
            LOG_E("SetupInvDevice fail!");
        }
    }
    else
    {
        LOG_E("platform init fail!");
    }
    rt_free(icm_serif);
    rt_free(icm42688p_dev);
    return NULL;

    // int16_t readbuffer[6];
    // for(uint16_t i=0; i<1000;i++)
    // {
    //     inv_io_hal_read_reg(&icm42688p_dev, MPUREG_ACCEL_DATA_X0_UI, (uint8_t *)readbuffer, 12);
    //     rt_kprintf("acc: %x \t %x \t %x \t gyro: %x \t %x \t %x \n",
    //     readbuffer[0],readbuffer[1],readbuffer[2],readbuffer[3],readbuffer[4],readbuffer[5]);

    //     rt_thread_mdelay(5);
    // }
}

static rt_size_t icm42688p_fetch_data(struct rt_sensor_device *sensor, void *buf, rt_size_t len)
{
    /**
     * @brief NOTE AXIS:
     *         X^
     *          |
     *  Y <-----|
     */
    int ret = 0;
    struct inv_icm426xx *icm42688p = (struct inv_icm426xx *)sensor->parent.user_data;
    struct rt_sensor_data *data = (struct rt_sensor_data *)buf;
    memset(buf, 0, sizeof(struct rt_sensor_data));
    if (sensor->info.type == RT_SENSOR_CLASS_ACCE)
    {
        uint8_t gyroBuffer[6];
        int16_t raw_ax, raw_ay, raw_az;

        ret = inv_icm426xx_read_reg(icm42688p, MPUREG_ACCEL_DATA_X0_UI, 6, gyroBuffer);

        raw_ax = (int16_t)(gyroBuffer[0] << 8 | gyroBuffer[1]);
        raw_ay = (int16_t)(gyroBuffer[2] << 8 | gyroBuffer[3]);
        raw_az = -(int16_t)(gyroBuffer[4] << 8 | gyroBuffer[5]);
        data->data.acce.x = (long)raw_ay * -1000 / accel_fs_factor;
        data->data.acce.y = (long)raw_ax * 1000 / accel_fs_factor;
        data->data.acce.z = (long)raw_az * -1000 / accel_fs_factor;
        data->type = RT_SENSOR_CLASS_ACCE;
        data->timestamp = rt_sensor_get_ts();
    }
    else if (sensor->info.type == RT_SENSOR_CLASS_GYRO)
    {
        /**
         * @brief NOTE AXIS:
         *         X^
         *          |
         *  Y <-----|
         * RIGHT HANDED
         */
        uint8_t gyroBuffer[6];
        int raw_gx, raw_gy, raw_gz;
        ret = inv_icm426xx_read_reg(icm42688p, MPUREG_GYRO_DATA_X0_UI, 6, gyroBuffer);
        raw_gx = -(int16_t)(gyroBuffer[0] << 8 | gyroBuffer[1]);
        raw_gy = (int16_t)(gyroBuffer[2] << 8 | gyroBuffer[3]);
        raw_gz = (int16_t)(gyroBuffer[4] << 8 | gyroBuffer[5]);
        data->data.gyro.x = (float)raw_gy * -1000.f / gyro_fs_factor;
        data->data.gyro.y = (float)raw_gx * -1000.f / gyro_fs_factor;
        data->data.gyro.z = (float)raw_gz * 1000.f / gyro_fs_factor;
        data->type = RT_SENSOR_CLASS_GYRO;
        data->timestamp = rt_sensor_get_ts();
    }

    if (ret != 0)
    {

        return -ret;
    }
    else
        return 1;
}
int icm42688p_event(inv_icm426xx_sensor_event_t *event)
{
    // event->sensor_mask.
    return 0;
}
static rt_err_t icm42688p_control(struct rt_sensor_device *sensor, int cmd, void *args)
{
    rt_err_t res = RT_EOK;
    struct inv_icm426xx *icm_dev = sensor->parent.user_data;
    if (icm_dev == RT_NULL)
        return -RT_ERROR;
    switch (cmd)
    {
    case RT_SENSOR_CTRL_SET_POWER:
        rt_uint32_t pctrl = (rt_uint32_t)args;
        if (pctrl == RT_SENSOR_POWER_DOWN)
        {
            LOG_D("icm42688p power down");
            res = inv_icm426xx_disable_fsync(icm_dev);
            res = inv_icm426xx_disable_accel(icm_dev);
            res = inv_icm426xx_disable_gyro(icm_dev);
        }
        else
        {
            res = icm42688p_ConfigureInvDevice(icm_dev);
            LOG_D("icm42688p power up");
        }
        break;
    case RT_SENSOR_CTRL_SET_ODR:
        if (sensor->info.type == RT_SENSOR_CLASS_ACCE)
        {
            res = inv_icm426xx_set_accel_frequency(icm_dev, (ICM426XX_GYRO_CONFIG0_ODR_t)args);
        }
        else if (sensor->info.type == RT_SENSOR_CLASS_GYRO)
        {
            res = inv_icm426xx_set_gyro_frequency(icm_dev, (ICM426XX_GYRO_CONFIG0_ODR_t)args);
        }
        break;
    case RT_SENSOR_CTRL_SET_RANGE:
        if (sensor->info.type == RT_SENSOR_CLASS_ACCE)
        {
            res = inv_icm426xx_set_accel_fsr(icm_dev, (ICM426XX_ACCEL_CONFIG0_FS_SEL_t)args);
        }
        else if (sensor->info.type == RT_SENSOR_CLASS_GYRO)
        {
            res = inv_icm426xx_set_gyro_fsr(icm_dev, (ICM426XX_ACCEL_CONFIG0_FS_SEL_t)args);
        }
        break;
    case RT_SENSOR_CTRL_GET_ID:
        res = inv_icm426xx_get_who_am_i(icm_dev, args);
        break;
    default:
        break;
    }
    return res;
}

static struct rt_sensor_ops icm42688p_ops =
    {
        icm42688p_fetch_data,
        icm42688p_control};

int rt_hw_icm42688p_sensor_reg(void)
{
    rt_uint8_t res = RT_EOK;
    rt_sensor_t icm_accel = RT_NULL, icm_gyro = RT_NULL;
    struct inv_icm426xx *icm42688p = icm42688p_create();
    if (icm42688p == RT_NULL)
    {
        LOG_E("icm42688p creat fial!");
        return -RT_ERROR;
    }
    icm_accel = rt_calloc(1, sizeof(struct rt_sensor_device));
    if (icm_accel == RT_NULL)
    {
        LOG_E("icm_accel creat fial!");
        return -RT_ERROR;
    }
    icm_accel->info.type = RT_SENSOR_CLASS_ACCE;
    icm_accel->info.vendor = RT_SENSOR_VENDOR_STM;
    icm_accel->info.model = "42688p";
    icm_accel->info.unit = RT_SENSOR_UNIT_MG;
    icm_accel->info.intf_type = RT_SENSOR_INTF_SPI;
    icm_accel->info.period_min = 1;
    icm_accel->info.fifo_max = 0;

    icm_accel->ops = &icm42688p_ops;
    res = rt_hw_sensor_register(icm_accel, "icm", RT_DEVICE_FLAG_RDWR, icm42688p);
    if (res != RT_EOK)
    {
        LOG_E("device register err code: %d", res);
        rt_free(icm_accel);
        return -RT_ERROR;
    }
    LOG_I("icm42688p acce registered\n");

    icm_gyro = rt_calloc(1, sizeof(struct rt_sensor_device));
    if (icm_gyro == RT_NULL)
    {
        LOG_E("icm_gyro creat fial!");
        return -RT_ERROR;
    }

    icm_gyro->info.type = RT_SENSOR_CLASS_GYRO;
    icm_gyro->info.vendor = RT_SENSOR_VENDOR_STM;
    icm_gyro->info.model = "42688p";
    icm_gyro->info.unit = RT_SENSOR_UNIT_MDPS;
    icm_gyro->info.intf_type = RT_SENSOR_INTF_SPI;
    icm_gyro->info.period_min = 1;
    icm_gyro->info.fifo_max = 0;

    icm_gyro->ops = &icm42688p_ops;
    res = rt_hw_sensor_register(icm_gyro, "icm", RT_DEVICE_FLAG_RDWR, icm42688p);
    if (res != RT_EOK)
    {
        LOG_E("device register err code: %d", res);
        rt_free(icm_gyro);
        return -RT_ERROR;
    }
    LOG_I("icm42688p gyro registered\n");
    return RT_EOK;
}
// int icm42688p_read_polling_init(void)
// {
//     return -1;
// }
// int icm42688p_gyro_read_polling(int32_t *gx, int32_t *gy, int32_t *gz)
// {
//     return 0;
// }
// int icm42688p_accel_read_polling(int32_t *ax, int32_t *ay, int32_t *az)
// {
//     return 0;
// }
ZINO_DEVICE_EXPORT(rt_hw_icm42688p_sensor_reg);
